
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include "fftwrap.h"

#define FRAMELEN 320
#define MICNUM 2

static void ifftshift(float *x, float *y, int N){
    int len = N/2; int i,j;
    for (i=len,j=0; i<N; i++,j++) y[j] = x[i];    
    for (i=0,j=len; i<=len; i++,j++) y[j] = x[i];    
}

static int argmax(float *v, int N, float *max){
    float x0 = v[0]; int idn = 0;
    for (int n=1; n<N; n++){
        if (v[n]>x0){
            x0 = v[n];
            idn = n;
        }
    }
    *max = v[idn];
    return idn;
}


int main(int argc, char *argv[])
{
    FILE *fidr = fopen(argv[1], "rb");
    fseek(fidr, 0, SEEK_END); int d2 = ftell(fidr);
    fseek(fidr, 0, SEEK_SET); int d1 = ftell(fidr);	
    int frames = (d2-d1)/sizeof(spx_int16_t)/FRAMELEN/MICNUM; 
    printf("total frames = %d\n", frames); 

    FILE *fidw = fopen("dump.dat", "wb");

    void *FFT = spx_fft_init(FRAMELEN*2);

    short *xbuf = (short *)malloc(sizeof(short)*FRAMELEN*2);
    
    float **micbuf = (float **)malloc(sizeof(float *)*MICNUM);
    for (int m=0; m<MICNUM; m++) {
        micbuf[m] = (float *)malloc(sizeof(float)*FRAMELEN*2);
    }

    float **Xbuf = (float **)malloc(sizeof(float *)*MICNUM);
    for (int m=0; m<MICNUM; m++) {
        Xbuf[m] = (float *)malloc(sizeof(float)*FRAMELEN*2);
    }

    float *XY = (float *)malloc(sizeof(float)*FRAMELEN*2);
    float *xybuf = (float *)malloc(sizeof(float)*FRAMELEN*2);
    
    for (int n=0; n<frames; n++) {
        fread(xbuf, sizeof(short), FRAMELEN*MICNUM, fidr);
        for (int m=0; m<MICNUM; m++) {
            memcpy(micbuf[m], micbuf[m] + FRAMELEN, sizeof(float)*FRAMELEN);
            for (int i=0; i<FRAMELEN; i++) {
                micbuf[m][FRAMELEN+i] = (float)xbuf[MICNUM*i+m];
            }
            spx_fft_float(FFT, micbuf[m], Xbuf[m]);               
        }
        /*dot*/ /*a+bi, c+di = (ac+bd) + (bc-ad)i*/ 
        float a,b,c,d; float ro, io; float modulus;
        memset(XY, 0, sizeof(float)*FRAMELEN*2);

        a = Xbuf[0][0]; b = 0.0f;
        c = Xbuf[1][0]; d = 0.0f;        
        ro = a*c + b*d; io = b*c - a*d;
        modulus = ro*ro + io*io;
        if (modulus<0.00001f) {
            XY[0] = 0.0f;
        }
        else {
            XY[0] = ro/sqrtf(modulus);
        }
        XY[0] = 0.0f;
                
        for (int nf=1; nf<FRAMELEN/*FRAMELEN*/; nf++) {
            a = Xbuf[0][2*nf-1]; b = Xbuf[0][2*nf];
            c = Xbuf[1][2*nf-1]; d = Xbuf[1][2*nf];
            ro = a*c + b*d; io = b*c - a*d;
            modulus = ro*ro + io*io;
            if (modulus<0.00001f) {
                XY[2*nf-1] = 0.0f; XY[2*nf] = 0.0f;
            }
            else {
                XY[2*nf-1] = ro/sqrtf(modulus);  XY[2*nf] = io/sqrtf(modulus);
            }                        
        }        

        XY[2*FRAMELEN-1] = 0.0f;

        spx_ifft_float(FFT, XY, xybuf);
        ifftshift(xybuf, XY, FRAMELEN*2);     
        for (int n=0; n<FRAMELEN*2; n++) {
            XY[n] = XY[n]/640.0f;
        }
        float maxv = 0.0f;
        int maxn = FRAMELEN - argmax(XY, FRAMELEN*2, &maxv);
        // if (maxv<0.25f) maxn = FRAMELEN;
        if (abs(maxn)>10) maxn = FRAMELEN;
        printf("frame = %d, max location = %d \n", n, maxn);   
        fwrite(XY, sizeof(float), FRAMELEN*2, fidw);        
    }

    spx_fft_destroy(FFT);


    // multi mic
    for (int m=0; m<MICNUM; m++) {
        free(micbuf[m]);
    }
    free(micbuf);
    // multi freq
    for (int m=0; m<MICNUM; m++) {
        free(Xbuf[m]);
    }
    free(Xbuf);
    // read buf
    free(xbuf);
    free(XY);
    free(xybuf);

    fclose(fidw);
    fclose(fidr);
    return 0;
}


